from ament_index_python.packages import get_package_share_path
import os
from launch import LaunchDescription
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.actions import Node
from launch.substitutions import Command


def generate_launch_description():
    # 拼接URDF文件路径
    urdf_path = os.path.join(get_package_share_path("bumperbot_description"), "urdf", "my_robot.urdf")

    rviz_config_path = os.path.join(get_package_share_path("bumperbot_description"), "rviz", "urdf_config.rviz")

    # 方法1：通过字典方式设置参数
    # 将URDF文件路径通过xacro命令处理后作为机器人描述参数
    params = {"robot_description": Command(["xacro ", urdf_path])}  # 使用Command执行xacro命令处理URDF文件，并将结果作为机器人描述参数
    # 方法2：通过ParameterValue对象设置参数（这里被注释掉了）
    # robot_description = ParameterValue(Command(["xacro ", urdf_path]), value_type=str)

    # 创建robot_state_publisher节点
    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[
            params  # 使用上面定义的params字典作为参数传递给robot_state_publisher节点
            # {"robot_description": robot_description}  # 方法2的参数设置方式，这里被注释掉了
        ]
    )

    # 创建joint_state_publisher_gui节点
    joint_state_publisher_gui_node = Node(
        package="joint_state_publisher_gui",
        executable="joint_state_publisher_gui"
    )

    # 创建rviz2节点
    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        arguments=["-d", rviz_config_path]
    )

    # 返回包含所有节点的LaunchDescription对象
    return LaunchDescription([
        robot_state_publisher_node,
        joint_state_publisher_gui_node,
        rviz2_node
    ])